#include "Renderer.h"
#include "Conio.h"
#include <iostream>
#include <thread>
#include "opencv2/core/core.hpp"

GLFWwindow * pWindow;
GLRenderer * pRenderer;
Hypothesis h;

float * val = &(h.thumb_Rotation[0]);

cv::Mat dataBase = cv::Mat::zeros(64,26,CV_32FC1);

static void key_callback(GLFWwindow* window, int key, int scancode, int action, int mods);

void main()
{
	pRenderer = new GLRenderer;
	pRenderer->initialize(pWindow,DRAW_FULL_COLOR);
	glfwSetKeyCallback(pRenderer->pWindow,key_callback);

	float tempoArray[26]={
		0,0,600,
		0,0,0,	
		50,20,18,12,	
		10,0,0,0,
		0,0,0,0,	
		-5,0,0,0,	
		-10,0,0,0};
	// POSE 0 - 7
	//float xoetay[26]={0,0,1000,	0,0,0,	50,20,18,12,	-3,0,0,0,	0,0,0,0,	2,0,0,0,	5,0,0,0};
	//float namtay[26]={0,0,1000,	0,0,0,	50,45,40,60,	-3,85,120,80,	0,85,110,75,	2,90,110,75,	5,90,100,65};
	//float trotay[26]={0,0,1000,	0,0,0,	50,45,40,60,	0,0,0,0,	0,90,110,75,	2,90,110,75,	5,90,100,65};
	//float goodtay[26]={0,0,1000,	0,0,0,	60,0,0,0,	-3,90,120,80,	0,90,110,75,	2,90,110,75,	5,90,100,65};
	//float linhtinh[26]={0,0,1000,	0,0,0,	60,0,0,0,	-3,0,0,0,	0,85,110,75,	2,90,110,75,	5,90,100,65};
	//float bopzu[26]={0,0,1000,	0,0,0,	60,20,30,60,	-3,20,90,60,	0,10,90,60,	2,20,90,90,	5,30,90,60};
	//float victory[26]={0,0,1000,	0,0,0,	60,20,30,60,	10,0,0,0,	-10,15,15,5,	-12,30,45,15,	-13,70,60,20};
	//float chumtay[26]={0,0,1000,	0,0,0,	60,90,0,0,	-3,70,15,5,	0,70,15,5,	2,70,15,5,	5,70,15,5};
	memcpy(&h,&tempoArray,26*sizeof(float));
	
	/*using namespace cv;
	std::vector<Vec3f> rotVec;
	rotVec.push_back(Vec3f(0,0,0));
	rotVec.push_back(Vec3f(70,0,0));
	rotVec.push_back(Vec3f(110,0,0));
	rotVec.push_back(Vec3f(150,0,0));
	rotVec.push_back(Vec3f(0,-45,0));
	rotVec.push_back(Vec3f(0,-135,0));
	rotVec.push_back(Vec3f(0,0,45));
	rotVec.push_back(Vec3f(0,0,-45));

	

	for (int i=0;i<8;i++)
	{
		float * ptr;
		switch (i)
		{
		case 0:	ptr=namtay;		break;
		case 1:	ptr=trotay;		break;
		case 2:	ptr=xoetay;		break;
		case 3:	ptr=goodtay;	break;
		case 4:	ptr=linhtinh;	break;
		case 5:	ptr=bopzu;		break;
		case 6:	ptr=victory;		break;
		case 7:	ptr=chumtay;		break;
		default:		break;
		}
		for (int j=0;j<8;j++)
		{
			int index = i*8+j;
			for (int k=0;k<26;k++)
			{
				dataBase.at<float>(index,k) = ptr[k];
			}
			dataBase.at<Vec3f>(index,1) = rotVec[j];
		}
	}*/

	//FileStorage fs = FileStorage("handPoseTemplate.yaml",FileStorage::WRITE);
	//fs<<"Pose"<<dataBase;
	//fs.release();

	pRenderer->rHand.handDOFs = &h.wirst_Position[0];
	pRenderer->calculatingHandCenter(cv::Point3f(pRenderer->rHand.handDOFs[0],pRenderer->rHand.handDOFs[1]+70,pRenderer->rHand.handDOFs[2]));
	pRenderer->rHand.lastTime = glfwGetTime();
	while (!glfwWindowShouldClose(pRenderer->pWindow))
	{
	pRenderer->rendering();
	}

	pRenderer->~GLRenderer();
}

#define GAIN_VAL 1

void printHypothesis()
{
	//printf("handDOF:%f\n",pRenderer->rHand.handDOFs[0]);
	//float *p = pRenderer->rHand.handDOFs;
	//printf("WRIST:\n %.1f; %.1f; %.1f\n %.1f; %.1f; %.1f\n",p[0],p[1],p[2],p[3],p[4],p[5]);
	//printf("THUMB: %.1f; %.1f; %.1f; %.1f\n",p[OFFSET_THUMB],p[OFFSET_THUMB+1],p[OFFSET_THUMB+2],p[OFFSET_THUMB+3]);
	//printf("FING1: %.1f; %.1f; %.1f; %.1f\n",p[OFFSET_FINGER1],p[OFFSET_FINGER1+1],p[OFFSET_FINGER1+2],p[OFFSET_FINGER1+3]);
	//printf("FING2: %.1f; %.1f; %.1f; %.1f\n",p[OFFSET_FINGER2],p[OFFSET_FINGER2+1],p[OFFSET_FINGER2+2],p[OFFSET_FINGER2+3]);
	//printf("FING3: %.1f; %.1f; %.1f; %.1f\n",p[OFFSET_FINGER3],p[OFFSET_FINGER3+1],p[OFFSET_FINGER3+2],p[OFFSET_FINGER3+3]);
	//printf("LITTLE: %.1f; %.1f; %.1f; %.1f\n",p[OFFSET_LITTLE],p[OFFSET_LITTLE+1],p[OFFSET_LITTLE+2],p[OFFSET_LITTLE+3]);

	static int m=0;
	const float * val = (float*)dataBase.ptr(m);
	for (int i=0;i<26;i++)
	{
		printf("%f;",val[i]);
	}
	printf("%\n");

	memcpy(&h,val,26*sizeof(float));
	m++;
}
static void key_callback(GLFWwindow* window, int key, int scancode, int action, int mods)
{
	if (action == GLFW_RELEASE)
	{
		return;
	}
	if (key == GLFW_KEY_ESCAPE && action == GLFW_PRESS)
		glfwSetWindowShouldClose(window, GL_TRUE);

	/* --- Select --- */
	if (key == GLFW_KEY_1)	val = &(h.thumb_Rotation[0]);
	if (key == GLFW_KEY_Q)	val = &(h.thumb_Rotation[1]);
	if (key == GLFW_KEY_A)	val = &(h.thumb_Rotation[2]);
	if (key == GLFW_KEY_Z)	val = &(h.thumb_Rotation[3]);

	if (key == GLFW_KEY_2)	val = &(h.finger1_Rotation[0]);
	if (key == GLFW_KEY_W)	val = &(h.finger1_Rotation[1]);
	if (key == GLFW_KEY_S)	val = &(h.finger1_Rotation[2]);
	if (key == GLFW_KEY_X)	val = &(h.finger1_Rotation[3]);

	if (key == GLFW_KEY_3)	val = &(h.finger2_Rotation[0]);
	if (key == GLFW_KEY_E)	val = &(h.finger2_Rotation[1]);
	if (key == GLFW_KEY_D)	val = &(h.finger2_Rotation[2]);
	if (key == GLFW_KEY_C)	val = &(h.finger2_Rotation[3]);

	if (key == GLFW_KEY_4)	val = &(h.finger3_Rotation[0]);
	if (key == GLFW_KEY_R)	val = &(h.finger3_Rotation[1]);
	if (key == GLFW_KEY_F)	val = &(h.finger3_Rotation[2]);
	if (key == GLFW_KEY_V)	val = &(h.finger3_Rotation[3]);

	if (key == GLFW_KEY_5)	val = &(h.little_Rotation[0]);
	if (key == GLFW_KEY_T)	val = &(h.little_Rotation[1]);
	if (key == GLFW_KEY_G)	val = &(h.little_Rotation[2]);
	if (key == GLFW_KEY_B)	val = &(h.little_Rotation[3]);

	/* WIRST ROTATION */
	if (key == GLFW_KEY_LEFT)	h.wirst_Rotation[1]+= GAIN_VAL;
	if (key == GLFW_KEY_RIGHT)	h.wirst_Rotation[1]-= GAIN_VAL;

	/* --- UP-DOWN --- */
	if (key == GLFW_KEY_UP)	val[0]+=GAIN_VAL;
	if (key == GLFW_KEY_DOWN)	val[0]-=GAIN_VAL;

	/*SHOW OR PRINT HYPO*/
	if (key == GLFW_KEY_SPACE)	printHypothesis();
}